#include "raster-fixed-math.h"

#include <limits.h>
/*
    Notes
    [1]: Sine and Cosine have a period of 2pi. Tangent has a period of pi.
    [2]: Accuracy drops dramatically for the tangent approximation once you pass pi/3 (just over FIXED_ONE)
        linear approximation using a very small lookup table is at least an improvement.
    [3]: Sine identities: sin(-x) = -sin(x), sin(x) = -sin(x - pi)
    [4]: Sine is symetrical across pi/2
    [5]: Tangent at +/- PI/2 is +/- infinity. This gives the best possible approximation of infinity.
         Failing to account for this could crash the program, as Cosine is 0 at +/- PI/2
         Also, an overflow error occurs at (PI/2)-1 which produces a value with a much smaller magnitude
            and different sign than should be, which could be a potentially show-stopping error.
            So I also return our infinity placeholder for that case.

    Sin, Asin, Tan and Atan approximations are based on Taylor series expansions of those functions.
    Cos and Acos just use Sin and Asin, respectively.
*/

#define fxmul(a, b) (Fixed)(((int64_t)(a) * (int64_t)(b)) >> 16)

Fixed __cdecl FixedSin(Fixed x)
{
    /* [3] */

    int sign = 1 | (x >> 31);
    x *= sign;
    /* [1] */
    if(x > FIXED_TWOPI)
        x = FixedMod(x, FIXED_TWOPI);
    /* [3] */
    if(x > FIXED_PI)
    {
        sign = -sign;
        x -= FIXED_PI;
    }
    /* [4] */
    if(x > FIXED_HALFPI)
        x = FIXED_HALFPI - (x - FIXED_HALFPI);
    Fixed xsq = FixedMul(x, x);
    Fixed xcb = FixedMul(xsq, x);
    Fixed xp5 = FixedMul(xcb, xsq);
    Fixed xp7 = FixedMul(xp5, xsq);
    Fixed ret = x - (xcb / 6) + (xp5 / 120) - (xp7 / 5040);
    return sign * ret;
}

Fixed __cdecl FixedCos(Fixed x)
{
    /* cos(x) = sin((pi/2) - x) */
    return FixedSin(FIXED_HALFPI - x);
}

Fixed __cdecl FixedTan(Fixed x)
{
    int sign = 1 | (x >> 31);
    x = x * sign;
    /* [1] */
    if(x > FIXED_PI)
        x = FixedMod(x, FIXED_PI);
    /* keep x between -pi/2 - pi/2 */
    if(x > FIXED_HALFPI)
    {
        x = -(x - FIXED_PI);
        sign = -sign;
    }
    /* [5] */
    if(FIXED_HALFPI - x <= 1)
        return sign * 0x7FFFFFFF; /* placeholder for infinity */
    /* [2] */
    if(x >= FIXED_ONE)
    {
        /* FIXME: this has to be really slow :(
            also, this still gets inaccurate when we get really close to x = PI/2
        */
        return sign * FixedDiv(FixedSin(x), FixedCos(x));
    }
    Fixed xsq = FixedMul(x, x);
    Fixed xcb = FixedMul(x, xsq);
    Fixed xp5 = FixedMul(xcb, xsq);
    Fixed xp7 = FixedMul(xsq, xp5);
    Fixed xp9 = FixedMul(xsq, xp7);
    Fixed ret = x + (xcb / 3) + ((2 * xp5) / 15) + ((17 * xp7) / 315) + ((62 * xp9) / 2835);
    return sign * ret;
}

Fixed __cdecl FixedAsin(Fixed x)
{
    if(IABS(x) > FIXED_ONE)
        return 0;

    Fixed xsq = FixedMul(x, x);
    Fixed xcb = FixedMul(x, xsq);
    Fixed xp5 = FixedMul(xcb, xsq);
    Fixed xp7 = FixedMul(xsq, xp5);
    return x + (xcb / 6) + ((3 * xp5) / 40) + ((15 * xp7) / 336);
}

Fixed __cdecl FixedAcos(Fixed x)
{
    return FIXED_HALFPI - FixedAsin(x);
}

static Fixed _calcFixedAtanGE1(Fixed x)
{
    Fixed xp2, xp3, xp5, xp7;
    x = FixedDiv(FIXED_ONE, x);
    xp2 = FixedMul(x, x);
    xp3 = FixedMul(xp2, x);
    xp5 = FixedMul(xp3, xp2);
    xp7 = FixedMul(xp5, xp2);

    return FIXED_HALFPI - x + (xp3 * 3) - (xp5 * 5) + (xp7 * 7);
}

static Fixed _calcFixedAtanL1(Fixed x)
{
    Fixed xsq = FixedMul(x, x);
    Fixed xcb = FixedMul(x, xsq);
    Fixed xp5 = FixedMul(xcb, xsq);
    Fixed xp7 = FixedMul(xsq, xp5);

    return x - (xcb / 3) + (xp5 / 5) - (xp7 / 7);
}

Fixed __cdecl FixedAtan(Fixed x)
{
    int sign = 1 | (x >> 31);
    x *= sign;
    if(x == 0x7FFFFFFF)
        return sign * FIXED_HALFPI;
    if(x < FIXED_ONE)
    {
        return sign * _calcFixedAtanL1(x);
    }
    return sign * _calcFixedAtanGE1(x);
}

/* babylonian method - very simple */
BEST_INLINE Fixed _calcSqrt(Fixed x)
{
    Fixed y = x / 2;
    Fixed r = FixedMul(y, y);
    Fixed error = r-x, lasterror = 0;
    int i = 0;
    while(error != lasterror && i < 16)
    {
        /* y = (y + (x / y)) / 2) */
        y = (y + FixedDiv(x, y)) >> 1;
        r = FixedMul(y, y);
        ++i;
        lasterror = error;
        error = r-x;
    }
    return y;
}

Fixed __cdecl FixedSqrt(Fixed x)
{
    if(x != FIXED_ONE && x > 0)
        return _calcSqrt(x);
    else if(x > 0) /* 1 or 0 */
        return x;
    return 0x80000000; /* NaN placeholder */
}

Fixed __cdecl FixedPowInt(Fixed base, int pow)
{
    Fixed ret = FIXED_ONE;
    int apow = IABS(pow);
    while(apow)
    {
        if(apow & 1)
            ret = FixedMul(ret, base);
        base = FixedMul(base, base);
        apow >>= 1; /* apow /= 2; */
    }
    if(pow < 0)
        return FixedDiv(FIXED_ONE, ret);
    return ret;
}

Fixed __cdecl FixedPow(Fixed base, Fixed pow)
{
    Fixed ret = FIXED_ONE;
    Fixed apow = IABS(pow);
    if(apow > 2 * FIXED_ONE)
    {
        ret = FixedPowInt(base, FixedToInt(apow));
        apow -= FixedFloor(apow);
    }
    while(apow <= FIXED_ONE)
    {
        apow <<= 1; /* apow *= 2 */
        base = FixedSqrt(base);
        if(apow & FIXED_ONE)
        {
            ret = FixedMul(ret, base);
        }

    }
    if(pow < 0)
        return FixedDiv(FIXED_ONE, ret);
    return ret;
}
